#include "usrframe.h"
#include "jkutil/iniparser.h"
#include "log/zuclog.h"

using axisgroup::UsrFrame;
using axisgroup::UsrFrameMgr;

static ZucPose zucPoseMult(const ZucPose& left, const ZucPose& right)
{
    PmPose l, r;
    l.tran = left.tran;
    PmRpy lrpy = {left.a, left.b, left.c};
    PmQuaternion lq;
    pmRpyQuatConvert(&lrpy, &lq);
    l.rot = lq;

    r.tran = right.tran;
    PmRpy rrpy = {right.a, right.b, right.c};
    PmQuaternion rq;
    pmRpyQuatConvert(&rrpy, &rq);
    r.rot = rq;

    PmPose ret;
    pmPosePoseMult(&l, &r, &ret);

    ZucPose out;
    out.tran = ret.tran;
    PmRpy rpy;
    pmQuatRpyConvert(&ret.rot, &rpy);
    out.a = rpy.y;
    out.b = rpy.p;
    out.c = rpy.r;
    out.u = 0.0;
    out.v = 0.0;
    out.w = 0.0;
    return out;
}

UsrFrame __usr_frames[kUsrFrameMaxCnt];

void axisgroup::UsrFrame::set_pose(const ZucPose& p)
{
    zuclog_info("USER_FRAME_INFO", "Set FRAME %d: %.2f %.2f %.2f %.2f %.2f %.2f", id_, p.tran.x, p.tran.y, p.tran.z, p.a, p.b, p.c);
    pose_ = p;
    is_changed_ = true;
}

const ZucPose& UsrFrame::world(int ref_id)
{
    if (ref_id < 0 || ref_id >= (int)UsrFrameMgr::MAX_USR_FRAME_NUM)
    {
        ref_id = 0;
    }
    auto ref = UsrFrameMgr::UsrFrameMgr::get_usr_frame(ref_id).pose();
    ref.a = ref.a / 180.0 * PM_PI;
    ref.b = ref.b / 180.0 * PM_PI;
    ref.c = ref.c / 180.0 * PM_PI;
    auto frame = pose_;
    frame.a = frame.a / 180.0 * PM_PI;
    frame.b = frame.b / 180.0 * PM_PI;
    frame.c = frame.c / 180.0 * PM_PI;
    world_pose_ = zucPoseMult(ref, pose_);
    world_pose_.a = world_pose_.a * 180.0 / PM_PI;
    world_pose_.b = world_pose_.b * 180.0 / PM_PI;
    world_pose_.c = world_pose_.c * 180.0 / PM_PI;
    return world_pose_;
}

std::string UsrFrame::pose_str() const
{
    return std::to_string(pose_.tran.x) + "," + std::to_string(pose_.tran.y) + "," + std::to_string(pose_.tran.z) + "," + std::to_string(pose_.a) + "," +
           std::to_string(pose_.b) + "," + std::to_string(pose_.c);
}

UsrFrame UsrFrameMgr::__usr_frame[UsrFrameMgr::MAX_USR_FRAME_NUM];
std::unique_ptr<Util::IniParser> UsrFrameMgr::ini_parser_;

UsrFrame& UsrFrameMgr::UsrFrameMgr::get_usr_frame(uint32_t id)
{
    if (id >= MAX_USR_FRAME_NUM)
    {
        return __usr_frame[0];
    }
    return __usr_frame[id];
}

int UsrFrameMgr::init_usr_frame(const char* filename)
{
    ini_parser_.reset();
    ini_parser_ = std::make_unique<Util::IniParser>(filename);

    for (uint32_t i = 0; i < MAX_USR_FRAME_NUM; i++)
    {
        std::string tag = "USR.FRAME_" + std::to_string(i);
        __usr_frame[i].id_ = i;

        std::vector<double> frame;
        ini_parser_->get(tag, frame, {0, 0, 0, 0, 0, 0}, -10000, 10000);
        ZucPose usr_frame = {};
        usr_frame.tran.x = frame[0];
        usr_frame.tran.y = frame[1];
        usr_frame.tran.z = frame[2];
        usr_frame.a = frame[3];
        usr_frame.b = frame[4];
        usr_frame.c = frame[5];
        __usr_frame[i].set_pose(usr_frame);

        zuclog_info("USR_FRAME_INFO",
                    "Load FRAME %d: %.2f %.2f %.2f %.2f",
                    i,
                    usr_frame.tran.x,
                    usr_frame.tran.y,
                    usr_frame.tran.z,
                    usr_frame.a,
                    usr_frame.b,
                    usr_frame.c);
        __usr_frame[i].is_changed_ = false;
    }
    return 0;
}

void UsrFrameMgr::save()
{
    for (uint32_t i = 0; i < MAX_USR_FRAME_NUM; i++)
    {
        if (!__usr_frame[i].is_changed_)
        {
            continue;
        }
        std::string tag = "USR.FRAME_" + std::to_string(i);
        ZucPose pose = __usr_frame[i].pose_;
        std::vector<double> frame = {pose.tran.x, pose.tran.y, pose.tran.z, pose.a, pose.b, pose.c};
        ini_parser_->set(tag, frame);
        __usr_frame[i].is_changed_ = false;
    }
    ini_parser_->save_ini();
}